package bodyPosition;

import java.util.Date;
import java.text.DateFormat;

import logging.LogToFile;

public class Position {

	private double[] position;
	private long time;
	private String bodyPlace;
	private double[] accel = null;
	//accel
	private double[] velocity = null;
	//log
	private LogToFile log = null;
	
	public Position(double[] a, long t, String s){
		position = a;
		time = t;		
		bodyPlace = s;
	}
	
	public Position(String s){
		this(new double[3],System.currentTimeMillis(), s);
	}
	
	public void setTime(long time) {
		this.time = time;
	}
	public long getTime() {
		return time;
	}
	
	public void setPosition(double[] position) {
		this.position = position;
	}
	public double[] getPosition() {
		return position;
	}
	
	public double[] getAcceleration(){
		return accel;
	}
	
	public void updateAcceleration(long t, double[] newPos){
		double dt = t-time;
		
		//change in pos
		double dx = newPos[0]-position[0];
		double dy = newPos[1]-position[1];
		double dz = newPos[2]-position[2];
		
		//velocity
		double vx = dx/dt;
		double vy = dy/dt;
		double vz = dz/dt;
		
		//check have a prev velocity
		if (velocity!=null){
			//change in velocity
			double dvx = velocity[0] - vx;
			double dvy = velocity[1] - vy;
			double dvz = velocity[2] - vz;
			
			//update acceleration 
			accel[0] = dvx/dt;
			accel[1] = dvy/dt;
			accel[2] = dvz/dt;
		}else{
			velocity = new double[3];
			accel = new double[3];
		}
		//update time
		time = t;
		//update pos
		position[0] = newPos[0];
		position[1] = newPos[1];
		position[2] = newPos[2];
		//update prevVel
		velocity[0] = vx;
		velocity[1] = vy;
		velocity[2] = vz;
		
	}
	
	public long timeInSeconds(){
		return (System.currentTimeMillis() - time) / 1000;
	}
	public String timeFormatted(){
		return DateFormat.getTimeInstance().format(new Date(time));
	}
	

	public synchronized LogToFile getLog(){
		if(log == null)
			log = new LogToFile(bodyPlace + "-" + System.currentTimeMillis());
		return log;
	}
	
	public String toString(){
		return String.format(timeFormatted()+" %s pos %d %d %d",bodyPlace,
									position[0],position[1],position[2]);
	}
	
}
